
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#define TAG "Main"

void motion_pid_rst(float p, float i, float d, float m) {}

float delta_seconds = 0;
void get_delta_seconds() {
  static uint32_t last_ms = 0;
  uint32_t cur_ms;
  mspm0_get_clock_ms(&cur_ms);

  delta_seconds = (cur_ms - last_ms) / 1000.0f;
  last_ms = cur_ms;
}

int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();
  OLED_Init();
  servo_init();
  motion_init();

  OLED_ShowString(0, 0, "Car liner2", 16);

  log(TAG, "Init Done!");

  float center_x = 0;
  float last_center_x = 0;
  float delta_x = 0;
  bool is_car_run = false;
  int16_t speed = 50;
  int16_t adjust_delay = 10;

  float kp = 65, ki = 0.00, kd = 0.0, m = 1000;

  motion_set_speed(speed);

  while (1) {
    if (is_car_run || 1) {
      get_delta_seconds();
      float err = line_patroler_get_error();
      motion_turn_pool(err, delta_seconds, 0.05);
      motion_move_pool(delta_seconds);
    }

    if (serial_is_available()) {
      char *str = serial_read_string();
      log(TAG, "%s", str);

      if (strstr(str, "start") != NULL) {
        OLED_ShowString(0, 0, "Running...", 16);

        motion_speed_ctrl(speed, speed);
        motion_pid_rst(kp, ki, kd, m);

        is_car_run = true;
      } else if (strstr(str, "stop") != NULL) {
        OLED_ShowString(0, 0, "Been stoped!", 16);
        motion_speed_ctrl(0, 0);
        is_car_run = false;
      } else if (strstr(str, "back") != NULL) {
        OLED_ShowString(0, 0, "Backing", 16);
        motion_speed_ctrl(-200, -200);
        is_car_run = false;
      } else if (strstr(str, "right") != NULL) {
        OLED_ShowString(0, 0, "Righting", 16);
        motion_speed_ctrl(100, -100);
        is_car_run = false;
      } else if (strstr(str, "left") != NULL) {
        OLED_ShowString(0, 0, "Lefting", 16);
        motion_speed_ctrl(-100, 100);
        is_car_run = false;
      }
      bool is_reset = false;
      char *ptr = NULL;
      ptr = strstr(str, "p=");
      if (ptr != NULL) {
        sscanf(ptr + 2, "%f", &kp);
        is_reset = true;
      }

      ptr = strstr(str, "i=");
      if (ptr != NULL) {
        sscanf(ptr + 2, "%f", &ki);
        is_reset = true;
      }

      ptr = strstr(str, "d=");
      if (ptr != NULL) {
        sscanf(ptr + 2, "%f", &kd);
        is_reset = true;
      }

      ptr = strstr(str, "m=");
      if (ptr != NULL) {
        sscanf(ptr + 2, "%f", &m);
        is_reset = true;
      }
      ptr = strstr(str, "a=");
      if (ptr != NULL) {
        sscanf(ptr + 2, "%d", (int *)&adjust_delay);
        is_reset = true;
      }
      ptr = strstr(str, "speed=");
      if (ptr != NULL) {
        sscanf(ptr + 6, "%d", (int *)&speed);
        log(TAG, "change speed=%d", speed);
        is_reset = true;
      }

      if (is_reset) {
        motion_pid_rst(kp, ki, kd, m);
        log(TAG, "change kp=%.2f ki=%.2f m=%.2f adjust_delay=%d", kp, ki, m,
               adjust_delay);
      }
    }
  }
}
